
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_stabilize.c
  * @author     baiyang
  * @date       2023-7-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static enum ModeNumber mstabilize_number(mode_base_const_t mode);
static const char *mstabilize_name(mode_base_const_t mode);
static const char *mstabilize_name4(mode_base_const_t mode);
static void mstabilize_update(mode_base_t mode);
static void mstabilize_run(mode_base_t mode);
static void mstabilize_stick_mixing_direct();
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_stabilize_ops = {
        .run                                = mstabilize_run,
        .mode_number                        = mstabilize_number,
        .name                               = mstabilize_name,
        .name4                              = mstabilize_name4,
        .update                             = mstabilize_update,
        .is_vtol_mode                       = NULL,
        .is_vtol_man_throttle               = NULL,
        .is_vtol_man_mode                   = NULL,
        .is_guided_mode                     = NULL,
        .allows_terrain_disable             = NULL,
        .does_automatic_thermal_switch      = NULL,
        .navigate                           = NULL,
        .allows_throttle_nudging            = NULL,
        .does_auto_navigation               = NULL,
        .does_auto_throttle                 = NULL,
        .mode_allows_autotuning             = NULL,
        .update_target_altitude             = NULL,
        .handle_guided_request              = NULL,
        .is_landing                         = NULL,
        .is_taking_off                      = NULL,
        ._enter                             = NULL,
        ._exit                              = NULL,
        ._pre_arm_checks                    = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mstabilize_ctor(struct ModeStabilize *mode_stabilize)
{
    mode_ctor(&mode_stabilize->mode, &mode_stabilize_ops);
}

// returns a unique number specific to this mode
static enum ModeNumber mstabilize_number(mode_base_const_t mode)
{
    (void)mode;
    return MN_STABILIZE;
}

// returns full text name
static const char *mstabilize_name(mode_base_const_t mode)
{
    (void)mode;
    return "STABILIZE";
}

// returns a string for this flightmode, exactly 4 bytes
static const char *mstabilize_name4(mode_base_const_t mode)
{
    (void)mode;
    return "STAB";
}

static void mstabilize_update(mode_base_t mode)
{
    fms.nav_roll_cd = 0;
    fms.nav_pitch_cd = 0;
}

static void mstabilize_run(mode_base_t mode)
{
    fms_stabilize_roll();
    fms_stabilize_pitch();
    mstabilize_stick_mixing_direct();
    fms_stabilize_yaw();
}

/*
  this gives the user control of the aircraft in stabilization modes, only used in Stabilize Mode
  to be moved to mode_stabilize.cpp in future
 */
static void mstabilize_stick_mixing_direct()
{
    if (!fms_stick_mixing_enabled()) {
        return;
    }
#if HAL_QUADPLANE_ENABLED
    if (!plane.quadplane.allow_stick_mixing()) {
        return;
    }
#endif
    float aileron = srv_channels_get_output_scaled(k_aileron);
    aileron = rc_stick_mixing(fms.channel_roll, aileron);
    srv_channels_set_output_scaled(k_aileron, aileron);

    float elevator = srv_channels_get_output_scaled(k_elevator);
    elevator = rc_stick_mixing(fms.channel_pitch, elevator);
    srv_channels_set_output_scaled(k_elevator, elevator);
}
/*------------------------------------test------------------------------------*/


